package com.example.auv_660.autocontrolalgorithm.assistantsensor;

import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;

import com.example.auv_660.MainActivity;

public class OrientationSensor implements SensorEventListener {
    private static OrientationSensor orientationSensor;
    private SensorManager sensorManager;
    private Sensor magnetic;
    private Sensor accelerometer;
    private MainActivity mainActivity = MainActivity.getInstance();

    private float[] accelerometerValues = new float[3];
    private float[] magneticFiledValues = new float[3];

    private float values[] = new float[3];

    public synchronized static OrientationSensor getInstance() {
        if (null == orientationSensor) {
            orientationSensor = new OrientationSensor();
        }
        return orientationSensor;
    }

    public float[] getValue()
    {
        return values;
    }

    public OrientationSensor()
    {
        if (mainActivity != null)
        {
            sensorManager = (SensorManager) mainActivity.getSystemService(Context.SENSOR_SERVICE);
        }

        if (sensorManager != null)
        {
            magnetic = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
            accelerometer = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
        }
        register();
    }

    public void register()
    {
        if (sensorManager != null)
        {
            sensorManager.registerListener(this, accelerometer, SensorManager.SENSOR_DELAY_FASTEST);
            sensorManager.registerListener(this, magnetic, SensorManager.SENSOR_DELAY_NORMAL);
        }
    }

    public void unregister()
    {
        if (sensorManager != null)
        {
            sensorManager.unregisterListener(this, accelerometer);
            sensorManager.unregisterListener(this, magnetic);
            sensorManager = null;
        }
    }

    @Override
    public void onSensorChanged(SensorEvent sensorEvent) {
        if (sensorEvent.sensor.getType() == Sensor.TYPE_ACCELEROMETER)
        {
            accelerometerValues = sensorEvent.values;
        }

        if (sensorEvent.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
        {
            magneticFiledValues = sensorEvent.values;
        }

        values = calculateOrientation(accelerometerValues, magneticFiledValues);

//        if (mainActivity != null)
//        {
//            mainActivity.updateAttitudeData(values);
//        }
    }

    private float[] calculateOrientation(float[] accelerometerValues, float[] magneticFiledValues)
    {
        float[] values = new float[3];
        float[] R = new float[9];
        SensorManager.getRotationMatrix(R, null, accelerometerValues, magneticFiledValues);
        SensorManager.getOrientation(R, values);
        values[0] = (float) Math.round(10 * (float) Math.toDegrees(values[0])) / 10;
        if (values[0] < 0)
        {
            values[0] = 360 + values[0];
        }
        values[1] = - (float) Math.round(10 * (float) Math.toDegrees(values[1])) / 10;
        values[2] = (float) Math.round(10 * (float) Math.toDegrees(values[2])) / 10;
        return values;
    }

    @Override
    public void onAccuracyChanged(Sensor sensor, int i) {

    }
}
